/*
 * This implementation of Priority Queue uses heap data structure
 * in order to push and pop elements into the queue.
 * The computational cost for both operations is: O(log(n))
 * uturan 19/03/2011

*/
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include "PQ.h"
#include "Common_Types.h"
#include "sys.h"

static U32 	getParent(int index);
static U32 	getLeftChild(int index);
static U32 	getRightChild(int index);
static void bubbleDown(PQ_STRUCT *q);
static void bubbleUp(PQ_STRUCT *q);
static U32 	getMaxChild(PQ_STRUCT *q, U32 i);

PQ_STRUCT * initPQ(U32 n, getpri getPriority, cmppri comparePriority)
{
	PQ_STRUCT *q;

    if ((q = malloc(sizeof(PQ_STRUCT)))== NULL)
        return NULL;

    if ((q->nodes = malloc((n + 1) * sizeof(void*))) == NULL) {
        free(q);
        return NULL;
    }

    q->currentSize = 1;
    q->availableSize = (n+1);
    q->getPriority = getPriority;
    q->comparePriority = comparePriority;
    return q;
}

void freeUpPQ(PQ_STRUCT *q)
{
    free(q->nodes);
    free(q);
}

STATUS insertNodePQ(PQ_STRUCT *q, void *node)
{
    //check whether queue is available to insert new item
    if ( q == NULL || (q->currentSize >= q->availableSize))
    	return ERROR;

    /* insert an item into PQ*/
    q->nodes[q->currentSize] = node;
    q->currentSize++;
    //re-adjust PQ structure
    bubbleUp(q);
    return OK;
}

void* popPQ(PQ_STRUCT *q)
{
	void* topNode;

    if ( q == NULL || q->currentSize - 1 == 0 )
        return NULL;

    topNode = q->nodes[1];
    //place last element to the top
    q->currentSize--;
    q->nodes[1] = q->nodes[q->currentSize];
    //place the first element in the correct location
    bubbleDown(q);

    return topNode;
}


void* peekPQ(PQ_STRUCT *q)
{
	void* topNode;
    if ( q == NULL || q->currentSize - 1 == 0 )
        return NULL;
    topNode = q->nodes[1];
    return topNode;
}

U32 getSizePQ(PQ_STRUCT *q)
{
    return (q->currentSize - 1);
}

static U32 getParent(int index)
{
	return (index/2);
}
static U32 getLeftChild(int index)
{
	return (2*index);
}
static U32 getRightChild(int index)
{
	return (2*index + 1);
}

static void bubbleUp(PQ_STRUCT *q)
{
	U32 i = q->currentSize -1 ;
    U32 parentNode = getParent(i);
    void* nodeToMove = q->nodes[i];
    TimeInternal* nodeToMovePriority = q->getPriority(nodeToMove);

    while( i > 1 && q->comparePriority( q->getPriority( q->nodes[parentNode]), nodeToMovePriority))
    {
    	q->nodes[i] = q->nodes[parentNode];
    	i = parentNode;
    	parentNode = getParent(i);
    }
    q->nodes[i] = nodeToMove;
}
static void bubbleDown(PQ_STRUCT *q)
{
	U32 i = 1;
    U32 childNode;
    void* nodeToMove = q->nodes[i];
    TimeInternal* nodeToMovePriority = q->getPriority(nodeToMove);

    while ( ((childNode = getMaxChild(q, i)) != 0 ) && q->comparePriority(nodeToMovePriority, q->getPriority(q->nodes[childNode])))
    {
        q->nodes[i] = q->nodes[childNode];
        i = childNode;
    }

    q->nodes[i] = nodeToMove;

}
static U32 getMaxChild(PQ_STRUCT *q, U32 i)
{
    U32 leftChild = getLeftChild(i);
    U32 rightChild = getRightChild(i);
    U32 maxChild = leftChild;
    if (leftChild >= q->currentSize)
        return 0;
    if ( rightChild < q->currentSize )
    {
    	if(q->comparePriority(q->getPriority(q->nodes[leftChild]), q->getPriority(q->nodes[rightChild])) )
    		maxChild = rightChild;
    }
    return maxChild;
}







